#ifndef ROBOTCMDNODE_CAMCAL_CENTER_H_
#define ROBOTCMDNODE_CAMCAL_CENTER_H_

/*****************************************************************************
** Includes
*****************************************************************************/
//ROS2
#include "rclcpp/rclcpp.hpp"
#include "pccmd_msg/msg/pc_cmd.hpp"
#include "param_msg/msg/param.hpp"
#include "rspos_msg/msg/rs_pos.hpp"
#include "dl_msg/msg/dl_msg.hpp"
#include "rsswitch_msg/msg/rs_switch.hpp"
#include "maptext_msg/msg/map_text.hpp"
#include "clawcmd_msg/msg/claw_cmd.hpp"

//CPP
#include <chrono>
#include <memory>
#include <functional>
#include <string>
#include <QByteArray>


class CamCalCenter;

/*****************************************************************************
  ** Class
  *****************************************************************************/

class RobotCmdNode : public rclcpp::Node
{
public:
    RobotCmdNode(CamCalCenter* rspc);
    virtual ~RobotCmdNode();

    void pubMapText(const std::string& name, int size, const QByteArray& data);

    void clawControl(uint8_t claw_dir, uint8_t claw_force);

    void sendCmd(const std::string& cmdname, const std::string& cmdcontent);

private:
    /*********************
      ** Subscriber y
      ** Publisher
      **********************/
    //rclcpp::Publisher<pccmd_msg::msg::PcCmd>::SharedPtr m_pubCmd;

    rclcpp::Publisher<pccmd_msg::msg::PcCmd>::SharedPtr m_pubCmd;

    rclcpp::Publisher<clawcmd_msg::msg::ClawCmd>::SharedPtr m_pubClawCmd;

    rclcpp::Subscription<param_msg::msg::Param>::SharedPtr m_subParam;
    void parammsg_callback(const param_msg::msg::Param::SharedPtr msg) const;

    rclcpp::Subscription<rspos_msg::msg::RsPos>::SharedPtr m_subRsPos;
    void rspos_callback(const rspos_msg::msg::RsPos::SharedPtr msg) const;

    rclcpp::Subscription<dl_msg::msg::DlMsg>::SharedPtr m_subDlMsg;
    void dlmsg_callback(const dl_msg::msg::DlMsg::SharedPtr msg) const;

    rclcpp::Subscription<rsswitch_msg::msg::RsSwitch>::SharedPtr m_subRsSwitch;
    void rsswitch_callback(const rsswitch_msg::msg::RsSwitch::SharedPtr msg) const;

    rclcpp::Publisher<maptext_msg::msg::MapText>::SharedPtr m_pubMapText;

    CamCalCenter* m_rspc;
};

#endif /* ROBOTCMDNODE_CAMCAL_CENTER_H_ */
